#ifndef CONDITIONALPARTICLEFILTER_H_
#define CONDITIONALPARTICLEFILTER_H_

#include "constants.h"
#include "Filter.h"
#include "MotionModel.h"
#include "CPFSensorModel.h"
#include "PersonDetector.h"

/**
 * There are Nr RobotSets, one for each robot pose.
 * Each RobotSet is a set of Ny configurations
 * Each configuration has keeps track of all the objects in the world -
 * 		there are M particles in each of the configurations
 */

typedef PoseArray Configuration;
typedef std::vector<Configuration> RobotSet;

class ConditionalParticleFilter: public Filter {
	int Nr, Ny;
	Robot config;


	void findPerRobotMeansAndStdsOfPersons(PoseArray &means, vector<DD> &stds, int r);
	void addNoise(MyPose pose, MyPose &noisy);
	void addNoise(Configuration &conf, Configuration &noisy);

public:
	int indexOfBestRobotPose, indexOfBestPersonConfiguration;
	/**
	 *
	 * this is the data-structure the encapsulates the estimates of where persons are.
	 * for each robot-pose,
	 * 	for each person-configuration
	 * 	  for each Person there exists a Pose
	 * 		vector< vector <vector<MyPose > > >
	 * [i] has Ny configurations
	 * [i][j] has M particles
	 * [i][j][m] is a particle
	 **/
	vector<RobotSet> robotEstimateOfPersonsConfigurations;
	ConditionalParticleFilter();
	virtual ~ConditionalParticleFilter();
	tf::Transform reSample(PoseArray &pa, WeightsForPoses &w, MotionModel &mm, CPFSensorModel &cpfsm,
			MATRIX &map, Robot &robot, OdomMove &om, const tf::Transform &currentTransform, STLPose &bestTargetPose, const int &M, SensorModel &sm);

	virtual tf::Transform reSample(PoseArray &pa, WeightsForPoses &w, MotionModel &mm,
			SensorModel &sm, MATRIX &map, Robot &robot, OdomMove &om,
			const tf::Transform &currentTransform) {
		ROS_ERROR("Wrong function called. Bad design choice");
		return tf::Transform();
	}

	PoseArray means;
	vector<DD> stds;

	void findMeansAndStdsOfPersons(const int &M);
};

#endif /* CONDITIONALPARTICLEFILTER_H_ */
